//
// Created by bill3000 on 2/11/21.
// Every thing need to do to start mechanic-related threads is to
// call start_mechanical_thread, then it's done!
//

#include <unistd.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <cstring>
#include <iostream>

#include "pthread.h"

#include "../fsm/macros.hpp"
#include "mechanic.hpp"
#include "../mq/queue.hpp"
#include "../log/logger.hpp"

//#define MECHANICAL_DEMO_SERVER

extern bool running;

//pthread_t Mechanic::mechanical_demo_server_thread_id;
pthread_t Mechanic::mechanical_control_thread_id;
pthread_t Mechanic::mechanical_emergency_thread_id;
Mechanic *Mechanic::mech;

void Mechanic::start_mechanical_thread(){
    int ret;
    mech = new Mechanic();

//    #ifdef MECHANICAL_DEMO_SERVER
//    ret = pthread_create(&mechanical_demo_server_thread_id, nullptr, mechanical_demo_server_thread, (void *)nullptr);
//    if(ret!=0) {
//        LOG(ERROR, "Failed to create mechanical demo server thread !");
//        exit(1);
//    }
//    #endif
    ret = pthread_create(&mechanical_control_thread_id, nullptr, mechanical_control_thread, (void *)mech);
    if(ret!=0) {
        LOG(ERROR, "Failed to create mechanical control thread !");
        exit(1);
    }
    ret = pthread_create(&mechanical_emergency_thread_id, nullptr, mechanical_emergency_thread, (void *)mech);
    if(ret!=0) {
        LOG(ERROR, "Failed to create mechanical emergency event thread !");
        exit(1);
    }
}

void Mechanic::wait_mechanical_thread() {
    pthread_join(mechanical_control_thread_id, nullptr);
    pthread_join(mechanical_emergency_thread_id, nullptr);

//    #ifdef MECHANICAL_DEMO_SERVER
//    pthread_join(mechanical_demo_server_thread_id, nullptr);
//    #endif
}

void Mechanic::stop_mechanical_thread() {
    pthread_cancel(mechanical_control_thread_id);
    pthread_cancel(mechanical_emergency_thread_id);

//    #ifdef MECHANICAL_DEMO_SERVER
//    pthread_cancel(mechanical_demo_server_thread_id);
//    #endif
}

void Mechanic::dispose() {
    delete mech;
    Queue::dispose();
}

void* Mechanic::mechanical_control_thread(void *data) {
    auto *m = (Mechanic *)data;
    m->init_network();
    m->mechanic_control();
    pthread_exit(nullptr);
}

void* Mechanic::mechanical_emergency_thread(void *data) {
    auto *m = (Mechanic *)data;
    m->mechanical_emergency();
    pthread_exit(nullptr);
}

Mechanic::Mechanic() {
    fd = -1;
    mq = Queue::get_instance();
}

void Mechanic::init_network() {
    fd = socket(AF_INET, SOCK_STREAM, 0);
    struct sockaddr_in sa = {0};
    memset(&sa, 0, sizeof(struct sockaddr_in));

    sa.sin_family = AF_INET;
    sa.sin_port = htons(8890);

    #ifdef MECHANICAL_DEMO_SERVER
    sa.sin_addr.s_addr = inet_addr("127.0.0.1");
    #else
    sa.sin_addr.s_addr = inet_addr("192.168.2.1");
    #endif

    //PLC IP:192.168.2.1
    int ret = connect(fd, (const struct sockaddr *)&sa,
                      sizeof(struct sockaddr_in));
    std::cout << "ret code:" << ret << std::endl;
    if (ret < 0) {
        LOG(ERROR, "Failed to connect to mechanical system !");
        exit(1);
    }
    LOG(INFO, "Connected to mechanical system successfully !");
}

void Mechanic::mechanic_control() {
    Message message;
    int ret;

    LOG(INFO, "Start mechanical control thread successfully !");
    while(running) {
        ret = mq->receive_message(M03, &message, MESSAGE_DIRECTION_SORTING_THREAD_TO_MECH);
        if (ret == -1) {
            LOG(ERROR, strerror(errno));
            continue;
        }
        handle(&message);
    }
}

void Mechanic::handle(Message *message) {
    unsigned int ctype = (message->data).ctype;
    int param = (message->data).param;
    switch(ctype) {
        case MECH_START_CONVEYER:
            start_conveyor(param);
            break;

        case MECH_STOP_CONVEYER:
            stop_conveyor(param);
            break;

        case MECH_PUSH_PUSHER:
            push_pusher(param);
            break;

        case MECH_RESET_MECHANIC_SYSTEM:
            reset_mechanical_system();
            break;

        default:
            LOG(ERROR, "Invalid command in mechanic control !");
    }
}

typedef struct ControlMessage {
    unsigned char command;  //命令码
    unsigned char channel;    //推杆号/皮带号等
    unsigned char param1;         //命令参数1
    unsigned char param2;         //命令参数2
    unsigned char param3;         //命令参数3
    unsigned char param4;         //命令参数1
    unsigned char param5;         //命令参数2
    unsigned char param6;         //命令参数3
} ControlMessage;

void Mechanic::start_conveyor(int which) const {
    ControlMessage cm;
    memset(&cm, 0, sizeof(cm));
    cm.command = 1;
    cm.channel = which;
    cm.param1 = 10;
    if (send(fd, &cm, sizeof(cm), 0) == -1) {
        LOG(ERROR, "Failed to start conveyer !");
    }
}

void Mechanic::stop_conveyor(int which) const {
    ControlMessage cm;
    memset(&cm, 0, sizeof(cm));
    cm.command = 2;
    cm.channel = which;
    if (send(fd, &cm, sizeof(cm), 0) == -1) {
        LOG(ERROR, "Failed to stop conveyer !");
    }
}

void Mechanic::push_pusher(int which) const {
    ControlMessage cm;
    memset(&cm, 0, sizeof(cm));
    cm.command = 3;
    cm.channel = which;
    cm.param1 = 0;
    cm.param2 = 0;
    if (send(fd, &cm, sizeof(cm), 0) == -1) {
        LOG(ERROR, "Failed to push pusher !");
    }
}

void Mechanic::reset_mechanical_system() const {
    ControlMessage cm;
    memset(&cm, 0, sizeof(cm));
    cm.command = 7;
    cm.channel = 0b00111111;
    if (send(fd, &cm, sizeof(cm), 0) == -1) {
        LOG(ERROR, "Failed to reset mechanical system !");
    }
}

void Mechanic::mechanical_emergency() const {
    int ret;
    char ss[200];
    ControlMessage cm;

    memset(&cm, 0, sizeof(cm));
    LOG(INFO, "Start mechanical emergency thread successfully !");
    while(running) {
        ret = recv(fd, &cm, sizeof(cm),0);
        if(ret <= 0) {
            LOG(ERROR, strerror(errno));
            continue;
        }
        sprintf(ss, "Received emergency, command: %d, channel: %d, param1: %d, param2: %d, param3: %d, param4: %d, param5: %d, param6: %d",
               (int)cm.command, (int)cm.channel, (int)cm.param1, (int)cm.param2,
               (int)cm.param3, (int)cm.param4, (int)cm.param5, (int)cm.param6);
        LOG(INFO, ss);
    }
}

void* Mechanic::mechanical_demo_server_thread(void *) {

    ControlMessage cm = {0};

    int listenSocket = socket(AF_INET,SOCK_STREAM,0);
    int on = 1;
    setsockopt(listenSocket, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on));
    struct sockaddr_in addrServer = {0},addrClient = {0};
    addrServer.sin_family = AF_INET;
    addrServer.sin_port = htons(8890);
    addrServer.sin_addr.s_addr = INADDR_ANY;

    int ret ;
//    int ret = bind(listenSocket,(struct sockaddr*)&addrServer,sizeof(struct sockaddr_in));
//    std::cout<<"bind code:" << ret <<"\nTCP/IP socket bind failed !"<<std::endl;
//
//    if (ret < 0) {
//        LOG(ERROR, "TCP/IP socket bind failed !");
//        exit(1);
//    }

    listen(listenSocket,5);

    int fds;
    int len = sizeof(struct sockaddr_in);
    LOG(INFO, "Waiting......");
    std::cout << "Waiting" << std::endl;
    fds = accept(listenSocket, (struct sockaddr*)&addrClient,
                reinterpret_cast<socklen_t *>(&len));
    char *ipStr = inet_ntoa(addrClient.sin_addr);
    std::string ss;
    ss.append("The Client IP Address is: ").append(ipStr);
    LOG(INFO, ss);

    while(running) {
        memset((void *)&cm, 0, sizeof(cm));
        ret = recv(fds,&cm,sizeof(cm),0);
        if(ret == 0) {
            ss = ss.erase();
            ss.append("Client ").append(ipStr).append("is disconnected!");
            LOG(INFO, ss);
            break;
        }
        if (ret < 0) break;
        switch(cm.command) {
            case 1:
                LOG(INFO, "启动皮带机");
                break;

            case 2:
                LOG(INFO, "停止皮带机");
                break;

            case 3:
                LOG(INFO, "启动分拣推杆");
                break;

            case 4:
                ss = ss.erase();
                ss.append("查询机电执行机构状态, ").append("返回执行机构状态报告, 命令码=5, 状态=0");
                LOG(INFO, ss);
                memset((void *)&cm, 0, sizeof(cm));
                cm.command = 5;
                cm.param1 = 0;
                send(fds, &cm ,sizeof(cm),0);
                break;

            default:
                LOG(INFO, "非法指令");
                break;
        }
    }
    close(fds);
    close(listenSocket);

    pthread_exit(nullptr);
}
